/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-12-06     MBW       the first version
 */
#include "uart_data.h"
#include "maths.h"
#include "filter.h"
#include "string.h"
#include "stdlib.h"
#include "stdio.h"
#include "drv_common.h"
#include "uart_config.h"
#include "board.h"
#include <rtdevice.h>
#include "niming_flow.h"
#define SAMPLE_UART_NAME       "uart5"      /* 串口设备名称 */
/* 用于接收消息的信号量 */
static rt_device_t serial;
static struct serial_configure config2 = RT_SERIAL_CONFIG_DEFAULT;  /* 初始化配置参数 */
_ano_of_st ano_of;
static uint8_t _datatemp[50];
static float check_time_ms[3];
/* 接收数据回调函数 */
static rt_err_t uart_input2(rt_device_t dev, rt_size_t size)
{
    char ucData;
    rt_device_read(serial, -1, &ucData, 1);
//    uart_GetOneByte(ucData);
    AnoOF_GetOneByte(ucData);
//      Flow_Get_for_cheap(ucData);
//    K210Prose(ucData);
}
void niming_flow_init(void *p)
{
    /* 查找串口设备 */
    serial = rt_device_find(SAMPLE_UART_NAME);
    if (!serial)
    {
        rt_kprintf("find %s failed!\n", SAMPLE_UART_NAME);
    }
    else rt_kprintf("find %s is ok!\n", SAMPLE_UART_NAME);
    /* step2：修改串口配置参数 */
//    config2.baud_rate = BAUD_RATE_19200;        //修改波特率为
    config2.baud_rate = BAUD_RATE_115200;        //修改波特率为
    config2.data_bits = DATA_BITS_8;           //数据位 8
    config2.stop_bits = STOP_BITS_1;           //停止位 1
    config2.bufsz     = 128;                   //修改缓冲区 buff size 为 128
    config2.parity    = PARITY_NONE;           //无奇偶校验位
    /* step3：控制串口设备。通过控制接口传入命令控制字，与控制参数 */
    rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config2);
    /* step4：打开串口设备。以中断接收及轮询发送模式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_FLAG_INT_RX);
    /* 以读写及中断接收方式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX);
    /* 设置接收回调函数 */
    rt_device_set_rx_indicate(serial, uart_input2);
   /* 发送字符串 */
//    rt_device_write(serial, 0, str, (sizeof(str) - 1));
    rt_thread_mdelay(10);
    rt_kprintf("flow_init uart6 is ok! \n");
}
INIT_APP_EXPORT(niming_flow_init);
void AnoOF_Check_State(float dT_s)
{
    u8 tmp[2];
    //Á¬½Ó¼ì²é
    if (check_time_ms[0] < 500)
    {
        check_time_ms[0]++;
        ano_of.link_sta = 1;
    }
    else
    {
        ano_of.link_sta = 0;
    }
    //Êý¾Ý¼ì²é1
    if (check_time_ms[1] < 500)
    {
        check_time_ms[1]++;
        tmp[0] = 1;
    }
    else
    {
        tmp[0] = 0;
    }
    //Êý¾Ý¼ì²é2
    if (check_time_ms[2] < 500)
    {
        check_time_ms[2]++;
        tmp[1] = 1;
    }
    else
    {
        tmp[1] = 0;
    }
    //ÉèÖÃ¹¤×÷×´Ì¬
    if (tmp[0] && tmp[1])
    {
        ano_of.work_sta = 1;
    }
    else
    {
        ano_of.work_sta = 0;
    }
}

//AnoOF_GetOneByteÊÇ³õ¼¶Êý¾Ý½âÎöº¯Êý£¬´®¿ÚÃ¿½ÓÊÕµ½Ò»×Ö½Ú¹âÁ÷Êý¾Ý£¬µ÷ÓÃ±¾º¯ÊýÒ»´Î£¬º¯Êý²ÎÊý¾ÍÊÇ´®¿ÚÊÕµ½µÄÊý¾Ý
//µ±±¾º¯Êý¶à´Î±»µ÷ÓÃ£¬×îÖÕ½ÓÊÕµ½ÍêÕûµÄÒ»Ö¡Êý¾Ýºó£¬»á×Ô¶¯µ÷ÓÃÊý¾Ý½âÎöº¯ÊýAnoOF_DataAnl
void AnoOF_GetOneByte(uint8_t data)
{
    static u8 _data_len = 0, _data_cnt = 0;
    static u8 rxstate = 0;

    if (rxstate == 0 && data == 0xAA)
    {
        rxstate = 1;
        _datatemp[0] = data;
    }
    else if (rxstate == 1 && (data == HW_TYPE || data == HW_ALL))  //0x61  0xFF
    {
        rxstate = 2;
        _datatemp[1] = data;
    }
    else if (rxstate == 2)
    {
        rxstate = 3;
        _datatemp[2] = data;
    }
    else if (rxstate == 3 && data < 250)
    {
        rxstate = 4;
        _datatemp[3] = data;
        _data_len = data;
        _data_cnt = 0;
    }
    else if (rxstate == 4 && _data_len > 0)
    {
        _data_len--;
        _datatemp[4 + _data_cnt++] = data;
        if (_data_len == 0)
            rxstate = 5;
    }
    else if (rxstate == 5)
    {
        rxstate = 6;
        _datatemp[4 + _data_cnt++] = data;
    }
    else if (rxstate == 6)
    {
        rxstate = 0;
        _datatemp[4 + _data_cnt] = data;
        //      DT_data_cnt = _data_cnt+5;
        //
        AnoOF_DataAnl(_datatemp, _data_cnt + 5); //
    }
    else
    {
        rxstate = 0;
    }
}
//AnoOF_DataAnlÎª¹âÁ÷Êý¾Ý½âÎöº¯Êý£¬¿ÉÒÔÍ¨¹ý±¾º¯ÊýµÃµ½¹âÁ÷Ä£¿éÊä³öµÄ¸÷ÏîÊý¾Ý
//¾ßÌåÊý¾ÝµÄÒâÒå£¬Çë²ÎÕÕÄäÃû¹âÁ÷Ä£¿éÊ¹ÓÃÊÖ²á£¬ÓÐÏêÏ¸µÄ½éÉÜ

static void AnoOF_DataAnl(uint8_t *data, uint8_t len)
{
    u8 check_sum1 = 0, check_sum2 = 0;
    if (*(data + 3) != (len - 6)) //ÅÐ¶ÏÊý¾Ý³¤¶ÈÊÇ·ñÕýÈ·
        return;
    for (u8 i = 0; i < len - 2; i++)
    {
        check_sum1 += *(data + i);
        check_sum2 += check_sum1;
    }
    if ((check_sum1 != *(data + len - 2)) || (check_sum2 != *(data + len - 1))) //ÅÐ¶ÏsumÐ£Ñé
        return;
    //================================================================================

    if (*(data + 2) == 0X51) //¹âÁ÷ÐÅÏ¢
    {
        if (*(data + 4) == 0) //Ô­Ê¼¹âÁ÷ÐÅÏ¢
        {
            ano_of.of0_sta = *(data + 5);
            ano_of.of0_dx = *(data + 6);
            ano_of.of0_dy = *(data + 7);
            ano_of.of_quality = *(data + 8);
        }
        else if (*(data + 4) == 1) //¸ß¶ÈÈÚºÏºó¹âÁ÷ÐÅÏ¢
        {
            ano_of.of1_sta = *(data + 5);
            ano_of.of1_dx = *((s16 *)(data + 6));
            ano_of.of1_dy = *((s16 *)(data + 8));
            ano_of.of_quality = *(data + 10);
            //
            check_time_ms[1] = 0;
            ano_of.of_update_cnt++;
        }
        else if (*(data + 4) == 2) //¹ßµ¼ÈÚºÏºó¹âÁ÷ÐÅÏ¢
        {
            ano_of.of2_sta = *(data + 5);
            ano_of.of2_dx = *((s16 *)(data + 6));
            ano_of.of2_dy = *((s16 *)(data + 8));
            ano_of.of2_dx_fix = *((s16 *)(data + 10));
            ano_of.of2_dy_fix = *((s16 *)(data + 12));
            ano_of.intergral_x = *((s16 *)(data + 14));
            ano_of.intergral_y = *((s16 *)(data + 16));
            ano_of.of_quality = *(data + 18);
            //
        }
    }
    else if (*(data + 2) == 0X34) //¸ß¶ÈÐÅÏ¢
    {
        ano_of.of_alt_cm = *((u32 *)(data + 7));
        //
        check_time_ms[2] = 0;
        ano_of.alt_update_cnt++;
    }
    else if (*(data + 2) == 0X01) //¹ßÐÔÊý¾Ý
    {
        ano_of.acc_data_x = *((s16 *)(data + 4));
        ano_of.acc_data_y = *((s16 *)(data + 6));
        ano_of.acc_data_z = *((s16 *)(data + 8));
        ano_of.gyr_data_x = *((s16 *)(data + 10));
        ano_of.gyr_data_y = *((s16 *)(data + 12));
        ano_of.gyr_data_z = *((s16 *)(data + 14));
        //shock_sta+16
    }
    else if (*(data + 2) == 0X04) //×ËÌ¬ÐÅÏ¢
    {
        //ËÄÔªÊý¸ñÊ½
        ano_of.quaternion[0] = (*((s16 *)(data + 4))) * 0.0001f;
        ano_of.quaternion[1] = (*((s16 *)(data + 6))) * 0.0001f;
        ano_of.quaternion[2] = (*((s16 *)(data + 8))) * 0.0001f;
        ano_of.quaternion[3] = (*((s16 *)(data + 10))) * 0.0001f;
    }
}

